#ifndef __EKF_H__
#define __EKF_H__

#include <iostream>
#include<stdio.h>
#include <sstream>
#include <fstream>
#include <string>
#include <vector>
#include "InsGpsEulerDirectEKF.hpp"
#include "math.hpp"

typedef double DataType;
#define g00 9.7940
#define pi 3.14159265

struct UavSensors
{
	DataType outcome[15];// = { 0 };
	DataType time_s;


	DataType gx_rps;
	DataType gy_rps;
	DataType gz_rps;
	DataType fx_mps2;
	DataType fy_mps2;
	DataType fz_mps2;

	int isYawUpdate;
	DataType yaw_rad;
	DataType yaw_deg;

	int isGPSUpdate;
	DataType Pn_m;
	DataType Pe_m;
	DataType h_msl_m;
	DataType Vn_mps;
	DataType Ve_mps;
	DataType Vd_mps;
};

typedef struct 
{
	int TimeStamp;
	double gx_rps; 
	double gy_rps;
	double gz_rps;
	double fx_mps2;
	double fy_mps2;
	double fz_mps2;
	int isGPSUpdate;
	double GNGGA_latitude;
	double GNGGA_longtitude;
	double GNGGA_height;
	double EKF_POS_E;
	double EKF_POS_N;
}EKF_RawData;

int prepare_data(const char* datapath);
int save_estvaule(const char* datapath);
void EKF_Update(EKF_RawData* EKFRAW);

#endif

